%% 验证sinistep_pgs函数

%% *算法验证函数*
function [] = vld_sinistep_pgs()
%VLD_SINISTEP_PGS 捷联惯性导航积分算法验证函数

clc;
clear;
par.samplePeriod = 0.02;
par.g = 9.7934752114984;
par.C = [0 0 0];
if ~exist('vld_sinistep_traj.mat','file')
    % 生成轨迹
    % 常量
    cst.sec2Rad = pi/3600/180;
    cst.deg2Rad = pi/180;
    cst_trajGen = load('earthconstants_WGS84.mat', 'omegaIE', 'RE', 'eSq', 'f');
    
    % 配置
    cfg_trajGen.enableTrajGen = true;
    cfg_trajGen.enableNavigation = false;
    cfg_trajGen.IMU.enableErr = true;
    cfg_trajGen.IMU.enableNoise = true;
    cfg_trajGen.IMU.ignore2OdErr = false;
    cfg_trajGen.IMU.quantizeOut = false;    
    cfg_trajGen.IMU.outFilePath = [];    
    if cfg_trajGen.IMU.quantizeOut
        cfg_trajGen.IMU.outPrecision = [];
    else
        cfg_trajGen.IMU.outPrecision = '%.16e';
    end
    
    % 输入
    in_trajGen.genFuncName = 'gentgi_navtest_regular';
    in_trajGen.genFuncInArg = {};
    
    % 参量
    par_trajGen.samplePeriod = par.samplePeriod;
    par_trajGen.g = par.g;
    par_trajGen.IMU.acc.dfBiasS = [33 33 33]' * par_trajGen.g * 1e-6; % 转换前单位μg，转换后单位m/s^2
    
    % 初始条件
    iniCon_trajGen.CVG = eye(3); % 初始姿态矩阵
    iniCon_trajGen.CBV = eye(3);
    iniCon_trajGen.vG = zeros(3, 1); % 初始速度
    iniCon_trajGen.pos = [30*cst.deg2Rad, 110*cst.deg2Rad, 0]'; % 纬度、经度、高度
    
    % 设置当前目录为轨迹发生器函数所在目录
    fileID = fopen('gentrajectory.m');
    fileFullPath = fopen(fileID);
    fclose(fileID);
    folderPath = getdirandfilenamefrompath(fileFullPath);
    cd(folderPath);
    % 运行轨迹仿真
    [out_trajGen, auxOut_trajGen] = gentrajectory(in_trajGen, cfg_trajGen, par_trajGen, iniCon_trajGen, cst_trajGen);
    save vld_sinistep_traj out_trajGen auxOut_trajGen
else
    load vld_sinistep_traj
end

%% *算法验证*
%% 0. 基本配置 四元数/WANAZM/高精度位置更新/无器件误差(用m文件，不用mex文件)
% 重命名文件
fileID = fopen('sinistep_pgs.m');
fileFullPath = fopen(fileID);
fclose(fileID);
folderPath = getdirandfilenamefrompath(fileFullPath);
if exist('sinistep_pgs.mexw64', 'file')
    movefile(strcat(folderPath, filesep, 'sinistep_pgs.mexw64'), strcat(folderPath, filesep, 'sinistep_pgs_temp.mexw64'));
end
% 配置
cfg.useDCMInAttCalc = false;
cfg.navCoordType = NavCoordType.WAN_AZM;
cfg.useHiResPosNSCal = true;
cfg.IMUErrFree = true;
hFig = {1, 2, 3};
vldfunction(cfg, out_trajGen, par, hFig);
if exist('sinistep_pgs_temp.mexw64', 'file')
    movefile(strcat(folderPath, filesep, 'sinistep_pgs_temp.mexw64'), strcat(folderPath, filesep, 'sinistep_pgs.mexw64')); % 将文件名还原
end
%% 1. 基本配置 四元数/WANAZM/高精度位置更新/无器件误差
cfg.useDCMInAttCalc = false;
cfg.navCoordType = NavCoordType.WAN_AZM;
cfg.useHiResPosNSCal = true;
cfg.IMUErrFree = true;
hFig = {hFig{1}+3, hFig{2}+3, hFig{3}+3};
vldfunction(cfg, out_trajGen, par, hFig);

%% 2. 配置1 DCM/WANAZM/高精度位置更新/无器件误差
cfg.useDCMInAttCalc = true;
cfg.navCoordType = NavCoordType.WAN_AZM;
cfg.useHiResPosNSCal = true;
cfg.IMUErrFree = true;
hFig = {hFig{1}+3, hFig{2}+3, hFig{3}+3};
vldfunction(cfg, out_trajGen, par, hFig);

%% 3. 配置2 四元数/GEO/高精度位置更新/无器件误差
cfg.useDCMInAttCalc = false;
cfg.navCoordType = NavCoordType.GEO;
cfg.useHiResPosNSCal = true;
cfg.IMUErrFree = true;
hFig = {hFig{1}+3, hFig{2}+3, hFig{3}+3};
vldfunction(cfg, out_trajGen, par, hFig);

%% 4. 配置3 四元数/FREEAZM/高精度位置更新/无器件误差
cfg.useDCMInAttCalc = false;
cfg.navCoordType = NavCoordType.FREE_AZM;
cfg.useHiResPosNSCal = true;
cfg.IMUErrFree = true;
hFig = {hFig{1}+3, hFig{2}+3, hFig{3}+3};
vldfunction(cfg, out_trajGen, par, hFig);

%% 5. 配置4 四元数/WANAZM/低精度位置更新/无器件误差
cfg.useDCMInAttCalc = false;
cfg.navCoordType = NavCoordType.WAN_AZM;
cfg.useHiResPosNSCal = false;
cfg.IMUErrFree = true;
hFig = {hFig{1}+3, hFig{2}+3, hFig{3}+3};
vldfunction(cfg, out_trajGen, par, hFig);

%% 6. 配置5 四元数/WANAZM/高精度位置更新/考虑垂直通道阻尼/无器件误差
cfg.useDCMInAttCalc = false;
cfg.navCoordType = NavCoordType.WAN_AZM;
cfg.useHiResPosNSCal = true;
cfg.IMUErrFree = true;
par.C = [2e-6, 0.00040308, 0.03];
hFig = {hFig{1}+3, hFig{2}+3, hFig{3}+3};
vldfunction(cfg, out_trajGen, par, hFig);

%% 7. 配置6 四元数/WANAZM/高精度位置更新/不考虑垂直通道阻尼/有器件误差
cfg.useDCMInAttCalc = false;
cfg.navCoordType = NavCoordType.WAN_AZM;
cfg.useHiResPosNSCal = true;
cfg.IMUErrFree = false;
par.C = [0, 0, 0];
hFig = {hFig{1}+3, hFig{2}+3, hFig{3}+3};
vldfunction(cfg, out_trajGen, par, hFig);

%% 8. 配置7 四元数/WANAZM/高精度位置更新/考虑垂直通道阻尼/有器件误差
cfg.useDCMInAttCalc = false;
cfg.navCoordType = NavCoordType.WAN_AZM;
cfg.useHiResPosNSCal = true;
cfg.IMUErrFree = false;
par.C = [2e-6, 0.00040308, 0.03];
hFig = {hFig{1}+3, hFig{2}+3, hFig{3}+3};
vldfunction(cfg, out_trajGen, par, hFig);
end

% *二、数值积分算法函数*
function [] = vldfunction(cfg, out_trajGen, par, hFig)
% 常量
cst_SINI = load('earthconstants_WGS84.mat', 'RE', 'f', 'mu', 'J2', 'J3', 'omegaIE');

% 配置
cfg_SINI.NSInLS = 1; % 低速解算周期内的中速解算周期数
cfg_SINI.HSInNS = 1; % 中速解算周期内的高速解算周期数
cfg_SINI.useDCMInAttCalc = cfg.useDCMInAttCalc;
cfg_SINI.useNormOrthoInAttCalc = true; % true表示在姿态解算中使用规范化（及正交化），false表示不使用
cfg_SINI.useHiResPosNSCal = cfg.useHiResPosNSCal;
cfg_SINI.useExtGravity = true; % true表示在使用外部提供的重力加速度，false表示使用重力模型计算重力
cfg_SINI.navCoordType = cfg.navCoordType;

% 参量
par_SINI.Tl = par.samplePeriod;
par_SINI.C = par.C;

% 输入量
if cfg.IMUErrFree
    in.accOut = out_trajGen.IMU.accOutErrFree;
    in.gyroOut = out_trajGen.IMU.gyroOutErrFree;
else
    in.accOut = out_trajGen.IMU.accOut;
    in.gyroOut = out_trajGen.IMU.gyroOut;
end

% 初始条件
iniCon_SINI.L_n = out_trajGen.pos(1, 1);
iniCon_SINI.L_n1 = iniCon_SINI.L_n;
iniCon_SINI.lambda_n = out_trajGen.pos(1, 2);
iniCon_SINI.lambda_n1 = iniCon_SINI.lambda_n;
iniCon_SINI.h_n = out_trajGen.pos(1, 3);
iniCon_SINI.h_n1 = iniCon_SINI.h_n;
iniCon_SINI.vN_m = out_trajGen.vG(1, 1:3)';
iniCon_SINI.vN_m1 = iniCon_SINI.vN_m;
iniCon_SINI.vN_n1 = iniCon_SINI.vN_m;
iniCon_SINI.CBL = [0 1 0; 1 0 0; 0 0 -1]*out_trajGen.CBG(:, :, 1);
iniCon_SINI.specVelInc_lx = in.accOut(1, :)'; % 0时刻之前的比速度增量
iniCon_SINI.angleInc_lx = in.gyroOut(1, :)';

% 运行
out_SINI.cnt = NaN(length(in.accOut), 4);
out_SINI.CNE = NaN(3, 3, length(in.accOut));
out_SINI.pos = NaN(length(in.accOut), 3);
out_SINI.vN = NaN(length(in.accOut), 3);
out_SINI.vG = NaN(length(in.accOut), 3);
out_SINI.CBL = NaN(3, 3, length(in.accOut));
out_SINI.CBG = NaN(3, 3, length(in.accOut));
out_SINI.qBL = NaN(length(in.accOut), 4);
out_SINI.att = NaN(length(in.accOut), 3);
out_SINI.psiTrue = NaN(length(in.accOut), 1);
out_SINI.alpha = NaN(length(in.accOut), 1);
out_SINI.gN = NaN(length(in.accOut), 3);
out_SINI.FCN = NaN(3, 3, length(in.accOut));
auxOut_SINI.altErr = NaN(length(in.accOut), 1);
auxOut_SINI.altErrIntGain = NaN(length(in.accOut), 1);
auxOut_SINI.coning = NaN(length(in.accOut), 3);
auxOut_SINI.sculling = NaN(length(in.accOut), 3);
auxOut_SINI.scrolling = NaN(length(in.accOut), 3);

clear('sinistep_pgs');
mode = int8(0);
hwb = waitbar(0, '开始解算...');
for i=1:length(in.accOut) % i=1时初始化，i>1时为解算周期
    waitbar(double(i)/double(length(in.accOut)), hwb, ...
        ['解算中...完成 ',int2str(100*double(i)/double(length(in.accOut))),' %']);
    mid_t = double(i)*par_SINI.Tl;
    if i==2
        mode = int8(1);
    end
    in_SINI = struct('specVelInc', in.accOut(i, :)', 'angleInc', in.gyroOut(i, :)',...
        'g', par.g,  'hAltm', out_trajGen.pos(i+1, 3), 'CNEExt', NaN(3), 'hExt', NaN,...
        'vNExt', NaN(3, 1), 'CBLExt', NaN(3), 'qBLExt', NaN(1, 4), ...
        'beta_m_ext', NaN(3, 1), 'DvScul_m_ext', NaN(3, 1), 'DrScrl_m_ext', NaN(3, 1));
    
    [tmp.out, tmp.auxOut] = sinistep_pgs(mode, in_SINI, cfg_SINI, par_SINI, iniCon_SINI, cst_SINI);
    out_SINI.t(i,:) = mid_t;
    out_SINI.cnt(i, :) = [tmp.out.cycCnt, tmp.out.HSCnt, tmp.out.NSCnt, tmp.out.LSCnt];
    out_SINI.CNE(:, :, i) = tmp.out.CNE;
    out_SINI.pos(i, :) = [tmp.out.L, tmp.out.lambda, tmp.out.h];
    out_SINI.vN(i, :) = tmp.out.vN';
    out_SINI.vG(i, :) = tmp.out.vG';
    out_SINI.CBL(:, :, i) = tmp.out.CBL;
    out_SINI.CBG(:, :, i) = tmp.out.CBG;
    out_SINI.qBL(i, :) = tmp.out.qBL;
    out_SINI.att(i, :) = [tmp.out.psi, tmp.out.theta, tmp.out.phi];
    out_SINI.psiTrue(i) = tmp.out.psiTrue;
    out_SINI.alpha(i) = tmp.out.alpha;
    out_SINI.gN(i, :) = tmp.out.gN';
    out_SINI.FCN(:, :, i) = tmp.out.FCN;
    auxOut_SINI.altErr(i) = tmp.auxOut.delh_n;
    auxOut_SINI.altErrIntGain(i) = tmp.auxOut.eVC3_n;
    auxOut_SINI.coning(i, :) = tmp.auxOut.beta_l';
    auxOut_SINI.sculling(i, :) = tmp.auxOut.DvScul_l';
    auxOut_SINI.scrolling(i, :) = tmp.auxOut.DrScrl_l';
end
close(hwb);

% 显示验证结果
out_trajGen.vG = out_trajGen.vG(:, :, 3);
out_trajGen.pos = out_trajGen.pos(:, :, 3);
out_sim = comparenavparam({out_SINI}, out_trajGen, cst_SINI.RE, hFig, {'导航'}, '轨迹');
disp('最大位置误差（m）');
disp(max(abs(out_sim.dPos{1}), [], 1));
disp('最大速度误差（m/s）');
disp(max(abs(out_sim.dvG{1}), [], 1));
disp('最大姿态误差（″）');
disp(max(abs(out_sim.phi{1}), [], 1));
disp('最大正交误差（″）');
disp(max(abs(out_sim.dOrth{1}), [], 1));
disp('最大规范误差（ppm）');
disp(max(abs(out_sim.dNorm{1}), [], 1));
end